#include "trajectory_generator_osqp.h"
#include <stdio.h>
// #include <ros/ros.h>
// #include <ros/console.h>
#include <iostream>
#include <fstream>
#include <string>

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"

// eigen
#include <Eigen/Dense>


using namespace std;    
using namespace Eigen;

TrajectoryGeneratorOsqp::TrajectoryGeneratorOsqp(){}
TrajectoryGeneratorOsqp::~TrajectoryGeneratorOsqp(){}

//define factorial function, input i, output i!
int TrajectoryGeneratorOsqp::Factorial(int x)
{
    int fac = 1;
    for(int i = x; i > 0; i--)
        fac = fac * i;
    return fac;
}

Eigen::MatrixXd TrajectoryGeneratorOsqp::PolyQPGeneration(const int d_order,           // the order of derivative
                                                              const Eigen::MatrixXd &Path, // waypoints coordinates (3d)
                                                              const Eigen::MatrixXd &Vel,  // boundary velocity
                                                              const Eigen::MatrixXd &Acc,  // boundary acceleration
                                                              const TimeAllocation &Time) // time allocation in each segment
{
    int p_order = 2 * d_order - 1;      // the order of polynomial
    int p_num1d = p_order + 1;          // the number of Coefficients in each segment
    int seg_num = Time.getTimeSegs();          // the number of segments

    cout << "p_numld= " << p_num1d << "seg_num= " << seg_num << "d_order= " << d_order << endl;
    MatrixXd PolyCoeff = MatrixXd::Zero(seg_num, 3 * p_num1d);     // position(x,y,z), so we need (3 * p_num1d) coefficients

    VectorXd Px(p_num1d * seg_num);     // coefficients in each axis
    VectorXd Py(p_num1d * seg_num);
    VectorXd Pz(p_num1d * seg_num);

    // enforce initial and final position,velocity and accleration, for higher order derivatives, just assume them be 0
    MatrixXd StartState(d_order, 3);
    MatrixXd EndState(d_order, 3);
    StartState.row(0) = Path.row(0); //起始点p
    StartState.row(1) = Vel.row(0);//起始点v
    StartState.row(2) = Acc.row(0);//起始点a
    EndState.row(0) = Path.row((Path.rows()-1));//终点p
    EndState.row(1) = Vel.row(1);//终点v
    EndState.row(2) = Acc.row(1);//终点a
    if(d_order == 4)
    {
        StartState.row(3) = VectorXd::Zero(3);  // 起点jerk
        EndState.row(3) = VectorXd::Zero(3);  //终点jerk
    }
    // cout << " StartState = " << endl;
    // cout << StartState << endl;
    // cout << " EndState = " << endl;
    // cout << EndState << endl;


    _Q = MatrixXd::Zero(p_num1d * seg_num, p_num1d * seg_num);

    for(int seg_index = 0; seg_index < seg_num; seg_index++)
    {
        // calculate Matrix Q
        _Q.block(seg_index*p_num1d, seg_index*p_num1d, p_num1d, p_num1d) = getQ(p_num1d, d_order, Time, seg_index);

    }
    cout << "_Q:" << endl << _Q << endl;

    // testQP();
    // cout << "test qp done." << endl;

    Px = OSQPCalCoeff1D(_Q, Path.col(0), StartState.col(0), EndState.col(0), Time, d_order);
    // cout << " Px = " << endl;
    // cout << Px << endl;
    Py = OSQPCalCoeff1D(_Q, Path.col(1), StartState.col(1), EndState.col(1), Time, d_order);
    Pz = OSQPCalCoeff1D(_Q, Path.col(2), StartState.col(2), EndState.col(2), Time, d_order);
    // cout << " Px = " << endl;
    // cout << Px << endl;
    // cout << " Py = " << endl;
    // cout << Py << endl;
    // cout << " Pz = " << endl;
    // cout << Pz << endl;

    for(int i = 0; i < seg_num; i++)
    {
        PolyCoeff.row(i).segment(0, p_num1d) = Px.segment(p_num1d*i, p_num1d);
        PolyCoeff.row(i).segment(p_num1d, p_num1d) = Py.segment(p_num1d*i, p_num1d);
        PolyCoeff.row(i).segment(2*p_num1d, p_num1d) = Pz.segment(p_num1d*i, p_num1d);
    }
    // cout << " PolyCoeff = " << endl;
    // cout << PolyCoeff << endl;

    return PolyCoeff;
}

Eigen::MatrixXd TrajectoryGeneratorOsqp::getQ(const int p_num1d, const int d_order, const TimeAllocation &Time, const int seg_index)
{
    #if 0
    // calculate Matrix Q_k of the seg_index-th segment
    MatrixXd Q_k = MatrixXd::Zero(p_num1d, p_num1d);
    for (int i = 0; i < p_num1d; i++)
    {
        for (int j = 0; j < p_num1d; j++)
        {
            if (i >= p_num1d - d_order && j >= p_num1d - d_order)
            {
                Q_k(i, j) = (Factorial(i) / Factorial(i - d_order)) * ((Factorial(j) / Factorial(j - d_order))) /
                            (i + j - 2 * d_order + 1) * Time(seg_index, i + j - 2 * d_order + 1); // Q of one segment
            }
        }
    }

    #else
    MatrixXd Q_k(p_num1d, p_num1d);

    // calculate Q_k of the k-th segment   with p decsending degree
    Q_k << 100800*Time(seg_index,7), 50400*Time(seg_index,6), 20160*Time(seg_index,5), 5040*Time(seg_index,4), 0, 0, 0, 0,
            50400*Time(seg_index,6), 25920*Time(seg_index,5), 10800*Time(seg_index,4), 2880*Time(seg_index,3), 0, 0, 0, 0,
            20160*Time(seg_index,5), 10800*Time(seg_index,4),  4800*Time(seg_index,3), 1440*Time(seg_index,2), 0, 0, 0, 0,
             5040*Time(seg_index,4),  2880*Time(seg_index,3),  1440*Time(seg_index,2),  576*Time(seg_index,1), 0, 0, 0, 0,
                                  0,                       0,                       0,                      0, 0, 0, 0, 0,
                                  0,                       0,                       0,                      0, 0, 0, 0, 0,
                                  0,                       0,                       0,                      0, 0, 0, 0, 0,
                                  0,                       0,                       0,                      0, 0, 0, 0, 0;


    // Q_k <<  0, 0, 0, 0, 0, 0, 0, 0,
    //         0, 0, 0, 0, 0, 0, 0, 0,
    //         0, 0, 0, 0, 0, 0, 0, 0,
    //         0, 0, 0, 0, 0, 0, 0, 0,
    //         0, 0, 0, 0,    576*Time(seg_index,1),  1440*Time(seg_index,2),   2880*Time(seg_index,3),   5040*Time(seg_index,4),
    //         0, 0, 0, 0,   1440*Time(seg_index,2),  4800*Time(seg_index,3),  10800*Time(seg_index,4),  20160*Time(seg_index,5),
    //         0, 0, 0, 0,   2880*Time(seg_index,3), 10800*Time(seg_index,4),  25920*Time(seg_index,5),  50400*Time(seg_index,6),
    //         0, 0, 0, 0,   5040*Time(seg_index,4), 20160*Time(seg_index,5),  50400*Time(seg_index,6), 100800*Time(seg_index,7);




    #endif

    // for(int i = 0;i < 8;++i)
    // {
    //     cout << "time(" << i << "): " <<  Time(seg_index,i) << endl;
    // }
    // cout << " Q_k = " << endl;
    // cout << Q_k << endl;

    return Q_k;
}

//下面是以降幂来编写的
std::pair<MatrixXd,VectorXd> TrajectoryGeneratorOsqp::getAbeq(const int d_order,           // the order of derivative
                                                const Eigen::VectorXd &WayPoints1D,
                                                const Eigen::VectorXd &StartState1D,
                                                const Eigen::VectorXd &EndState1D,
                                                const TimeAllocation &Time) // time allocation in each segment
{
    int p_order = 2 * d_order - 1;      // the order of polynomial
    int p_num1d = p_order + 1;          // the number of Coefficients in each segment
    int seg_num = Time.getTimeSegs();          // the number of segments

    int n_all_poly = seg_num * p_num1d;//the number of all polynomial coefficients

    int all_rows = 4*2 + 5*(seg_num-1);
    //define matrix for assembling all matrix block
    MatrixXd Aeq = MatrixXd::Zero(all_rows, n_all_poly);
    VectorXd beq = VectorXd::Zero(all_rows);
    
    int Aeq_rows = 0;

    //p,v,a,j constraint in start, 
    MatrixXd Aeq_start = MatrixXd::Zero(4, n_all_poly);//为了保证矩阵的列数一致，后面都补0
    Eigen::MatrixXd m(4,p_num1d);
    m << 0, 0, 0, 0, 0, 0, 0, 1,
         0, 0, 0, 0, 0, 0, 1, 0,
         0, 0, 0, 0, 0, 2, 0, 0,
         0, 0, 0, 0, 6, 0, 0, 0;

    // m <<  1,           0,           0,           0,           0,           0,           0,           0,
    //       0,           1,           0,           0,           0,           0,           0,           0,
    //       0,           0,           2,           0,           0,           0,           0,           0,
    //       0,           0,           0,           6,           0,           0,           0,           0;


    VectorXd beq_start = VectorXd::Zero(4); 
    //write expression of Aeq_start and beq_start
    Aeq_start.block(0,0,4,p_num1d) = m;
    beq_start = StartState1D;// p,v,a,j
    //add Aeq_start and beq_start
    Aeq.block(Aeq_rows,0,4,n_all_poly) = Aeq_start;
    beq.segment(Aeq_rows,4) = beq_start;
    Aeq_rows += 4;
    cout << "Aeq_start:" << endl << Aeq_start << endl;
    cout << "beq_start:" << endl << beq_start << endl;

    //p,v,a,j constraint in end
    MatrixXd Aeq_end = MatrixXd::Zero(4, n_all_poly);//为了保证矩阵的列数一致，前面都补0
    double seg_index = seg_num-1;// time of the last trajectory 每段轨迹使用相对时间
    Eigen::MatrixXd n(4,p_num1d);
    n <<        Time(seg_index,7),     Time(seg_index,6),    Time(seg_index,5),    Time(seg_index,4),   Time(seg_index,3),   Time(seg_index,2), Time(seg_index,1), 1,
              7*Time(seg_index,6),   6*Time(seg_index,5),  5*Time(seg_index,4),  4*Time(seg_index,3), 3*Time(seg_index,2), 2*Time(seg_index,1),                 1, 0,
             42*Time(seg_index,5),  30*Time(seg_index,4), 20*Time(seg_index,3), 12*Time(seg_index,2), 6*Time(seg_index,1),                   2,                 0, 0,
            210*Time(seg_index,4), 120*Time(seg_index,3), 60*Time(seg_index,2), 24*Time(seg_index,1),                   6,                   0,                 0, 0;

    // n <<       1, Time(seg_index,1),   Time(seg_index,2),   Time(seg_index,3),    Time(seg_index,4),    Time(seg_index,5),    Time(seg_index,6),    Time(seg_index,7),  //末端位置的状态量导数
    //            0,              1   , 2*Time(seg_index,1), 3*Time(seg_index,2),  4*Time(seg_index,3),  5*Time(seg_index,4),  6*Time(seg_index,5),  7*Time(seg_index,6),
    //            0,              0   ,              2     , 6*Time(seg_index,1), 12*Time(seg_index,2), 20*Time(seg_index,3), 30*Time(seg_index,4), 42*Time(seg_index,5),
    //            0,              0   ,              0     ,              6     , 24*Time(seg_index,1), 60*Time(seg_index,2),120*Time(seg_index,3),210*Time(seg_index,4);

    VectorXd beq_end = VectorXd::Zero(4); 
    //write expression of Aeq_end and beq_end
    Aeq_end.block(0, n_all_poly-p_num1d,4,p_num1d) = n;
    beq_end = EndState1D;// p,v,a,j
    //add Aeq_end and beq_end
    Aeq.block(Aeq_rows,0,4,n_all_poly) = Aeq_end;
    beq.segment(Aeq_rows,4) = beq_end;
    Aeq_rows += 4;
    cout << "Aeq_end:" << endl << Aeq_end << endl;
    cout << "beq_end:" << endl << beq_end << endl;

    if(seg_num > 1)
    {
        // position constrain in all middle waypoints
        MatrixXd Aeq_wp = MatrixXd::Zero(seg_num-1, n_all_poly);
        VectorXd beq_wp = VectorXd::Zero(seg_num-1);
        // write expression of Aeq_wp and beq_wp
        for(int midwp_index = 0;midwp_index < seg_num-1;++midwp_index)
        {
            int index = 8 * midwp_index;
            
            Eigen::MatrixXd tmp(1,p_num1d);
            tmp << Time(midwp_index,7), Time(midwp_index,6), Time(midwp_index,5), Time(midwp_index,4), Time(midwp_index,3), Time(midwp_index,2), Time(midwp_index,1), 1;
            Aeq_wp.block(midwp_index,index,1,p_num1d) = tmp;// the end of previous segment

            beq_wp(midwp_index) = WayPoints1D(midwp_index+1);//WayPoints1D(0)是起点
        }
        Aeq.block(Aeq_rows,0,seg_num-1,n_all_poly) = Aeq_wp;
        beq.segment(Aeq_rows,seg_num-1) = beq_wp;
        Aeq_rows += seg_num-1;
        cout << "Aeq_wp:" << endl << Aeq_wp << endl;
        cout << "beq_wp:" << endl << beq_wp << endl;

        // position continuity constrain between each 2 segments
        MatrixXd Aeq_con_p = MatrixXd::Zero(seg_num-1, n_all_poly);
        VectorXd beq_con_p = VectorXd::Zero(seg_num-1);
        // write expression of Aeq_con_p and beq_con_p
        for(int con_p_index = 0;con_p_index < seg_num-1;++con_p_index)
        {
            int index = 8 * con_p_index;
            
            Eigen::MatrixXd tmp(1,p_num1d);
            tmp << Time(con_p_index,7), Time(con_p_index,6), Time(con_p_index,5), Time(con_p_index,4), Time(con_p_index,3), Time(con_p_index,2), Time(con_p_index,1), 1;
            Aeq_con_p.block(con_p_index,index,1,p_num1d) = tmp;// the end of previous segment
            Eigen::MatrixXd tmp1(1,p_num1d);
            tmp1 << 0, 0, 0, 0, 0, 0, 0, -1;
            Aeq_con_p.block(con_p_index,index+8,1,p_num1d) = tmp1;// the begin of next segment

        }
        // beq_con_p is a zero vector
        Aeq.block(Aeq_rows,0,seg_num-1,n_all_poly) = Aeq_con_p;
        // beq.segment(Aeq_rows,seg_num-1) = beq_con_p;
        Aeq_rows += seg_num-1;
        cout << "Aeq_con_p:" << endl << Aeq_con_p << endl;
        cout << "beq_con_p:" << endl << beq_con_p << endl;

        // velocity continuity constrain between each 2 segments
        MatrixXd Aeq_con_v = MatrixXd::Zero(seg_num-1, n_all_poly);
        VectorXd beq_con_v = VectorXd::Zero(seg_num-1);
        // write expression of Aeq_con_v and beq_con_v
        for(int con_v_index = 0;con_v_index < seg_num-1;++con_v_index)
        {

            int index = 8 * con_v_index;
            
            Eigen::MatrixXd tmp(1,p_num1d);
            tmp << 7*Time(con_v_index,6), 6*Time(con_v_index,5), 5*Time(con_v_index,4), 4*Time(con_v_index,3), 3*Time(con_v_index,2), 2*Time(con_v_index,1), 1, 0;
            Aeq_con_v.block(con_v_index,index,1,p_num1d) = tmp;// the end of previous segment
            Eigen::MatrixXd tmp1(1,p_num1d);
            tmp1 << 0, 0, 0, 0, 0, 0, -1, 0;
            Aeq_con_v.block(con_v_index,index+8,1,p_num1d) = tmp1;// the begin of next segment
        }
        // beq_con_v is a zero vector
        Aeq.block(Aeq_rows,0,seg_num-1,n_all_poly) = Aeq_con_v;
        // beq.segment(Aeq_rows,seg_num-1) = beq_con_v;
        Aeq_rows += seg_num-1;
        cout << "Aeq_con_v:" << endl << Aeq_con_v << endl;
        cout << "beq_con_v:" << endl << beq_con_v << endl;

        // acceleration continuity constrain between each 2 segments
        MatrixXd Aeq_con_a = MatrixXd::Zero(seg_num-1, n_all_poly);
        VectorXd beq_con_a = VectorXd::Zero(seg_num-1);
        // write expression of Aeq_con_a and beq_con_a
        for(int con_a_index = 0;con_a_index < seg_num-1;++con_a_index)
        {
            int index = 8 * con_a_index;

            Eigen::MatrixXd tmp(1,p_num1d);
            tmp << 42*Time(con_a_index,5), 30*Time(con_a_index,4), 20*Time(con_a_index,3), 12*Time(con_a_index,2), 6*Time(con_a_index,1), 2, 0, 0;
            Aeq_con_a.block(con_a_index,index,1,p_num1d) = tmp;// the end of previous segment
            Eigen::MatrixXd tmp1(1,p_num1d);
            tmp1 << 0, 0, 0, 0, 0, -2, 0, 0;
            Aeq_con_a.block(con_a_index,index+8,1,p_num1d) = tmp1;// the begin of next segment

        }
        // beq_con_a is a zero vector
        Aeq.block(Aeq_rows,0,seg_num-1,n_all_poly) = Aeq_con_a;
        // beq.segment(Aeq_rows,seg_num-1) = beq_con_a;
        Aeq_rows += seg_num-1;
        cout << "Aeq_con_a:" << endl << Aeq_con_a << endl;
        cout << "beq_con_a:" << endl << beq_con_a << endl;

        // jerk continuity constrain between each 2 segments
        MatrixXd Aeq_con_j = MatrixXd::Zero(seg_num-1, n_all_poly);
        VectorXd beq_con_j = VectorXd::Zero(seg_num-1);
        // write expression of Aeq_con_j and beq_con_j
        for(int con_j_index = 0;con_j_index < seg_num-1;++con_j_index)
        {
            int index = 8 * con_j_index;

            Eigen::MatrixXd tmp(1,p_num1d);
            tmp << 210*Time(con_j_index,4), 120*Time(con_j_index,3), 60*Time(con_j_index,2), 24*Time(con_j_index,1), 6, 0, 0, 0;
            Aeq_con_j.block(con_j_index,index,1,p_num1d) = tmp;// the end of previous segment
            Eigen::MatrixXd tmp1(1,p_num1d);
            tmp1 << 0, 0, 0, 0, -6, 0, 0, 0;
            Aeq_con_j.block(con_j_index,index+8,1,p_num1d) = tmp1;// the begin of next segment

        }
        // beq_con_j is a zero vector
        Aeq.block(Aeq_rows,0,seg_num-1,n_all_poly) = Aeq_con_j;
        // beq.segment(Aeq_rows,seg_num-1) = beq_con_j;
        cout << "Aeq_con_j:" << endl << Aeq_con_j << endl;
        cout << "beq_con_j:" << endl << beq_con_j << endl;
    }

    return std::make_pair(Aeq,beq);
}

Eigen::VectorXd TrajectoryGeneratorOsqp::OSQPCalCoeff1D(const Eigen::MatrixXd &Q,
                                                        const Eigen::VectorXd &WayPoints1D,
                                                        const Eigen::VectorXd &StartState1D,
                                                        const Eigen::VectorXd &EndState1D,
                                                        const TimeAllocation &Time,
                                                        const int d_order)
{
    cout << "enter into OSQPCalCoeff1D" << endl;
    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian; //P or H
    Eigen::VectorXd gradient;  //f or q
    Eigen::SparseMatrix<double> linearMatrix;  //A
    Eigen::VectorXd lowerBound; //l
    Eigen::VectorXd upperBound; //u

    gradient = VectorXd::Zero(Q.rows());
    std::cout << "gradient:" << endl << gradient << endl;
    std::cout << "Q:" << endl << Q << endl;
    hessian = Q.sparseView();
    std::cout << "hessian:" << endl << hessian << endl;

    std::pair<MatrixXd,VectorXd> Abeq = getAbeq(d_order,WayPoints1D,StartState1D,EndState1D,Time);

    linearMatrix = Abeq.first.sparseView();
    lowerBound = Abeq.second;
    upperBound = Abeq.second;


    cout << "Abeq:" << endl << Abeq.first << endl;
    cout << "lowerBound:" << endl << lowerBound << endl;

    int NumberOfVariables = Abeq.first.cols();
    int NumberOfConstraints = Abeq.first.rows();

    cout << "NumberOfVariables=" << NumberOfVariables << " NumberOfConstraints=" << NumberOfConstraints << endl;
    // instantiate the solver
    OsqpEigen::Solver solver;

    // settings
    solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    //矩阵A为m*n矩阵
    solver.data()->setNumberOfVariables(NumberOfVariables); //设置A矩阵的列数，即n
    solver.data()->setNumberOfConstraints(NumberOfConstraints); //设置A矩阵的行数，即m

    if(!solver.data()->setHessianMatrix(hessian))//设置P矩阵
    {
        cout << "[error] setHessianMatrix" << endl;
    }
    if(!solver.data()->setGradient(gradient)) //设置q or f矩阵。当没有时设置为全0向量
    {
        cout << "[error] setGradient" << endl;
    }
    if(!solver.data()->setLinearConstraintsMatrix(linearMatrix))//设置线性约束的A矩阵
    {
        cout << "[error] setLinearConstraintsMatrix" << endl;
    }
    if(!solver.data()->setLowerBound(lowerBound))//设置下边界
    {
        cout << "[error] setLinearConstraintsMatrix" << endl;
    }

    if(!solver.data()->setUpperBound(upperBound))//设置上边界
    {
        cout << "[error] setUpperBound" << endl;
    }

    cout << "QPSolution1: " << endl;
    // instantiate the solver
    if(!solver.initSolver())
    {
        cout << "[error] initSolver" << endl;
    }

    Eigen::VectorXd QPSolution;

    cout << "QPSolution2: " << endl;

    // solve the QP problem
    if(!solver.solve())
    {
        cout << "[error] solve" << endl;
    }
    cout << "QPSolution3: " << endl;
    // get the controller input
    QPSolution = solver.getSolution();

    // int first = 0;
    // int last = QPSolution.size();
    // while ((first!=last)&&(first!=--last))
    // {
    //     double tmp = QPSolution(first);
    //     QPSolution(first) = QPSolution(last);
    //     QPSolution(last) = tmp;

    //     ++first;
    // }


    cout << "QPSolution: " << endl << QPSolution << endl;

    //取反变成升序
    // return QPSolution.reverse();
    return QPSolution;
}


bool TrajectoryGeneratorOsqp::testQP()
{


    // allocate QP problem matrices and vectores
    Eigen::SparseMatrix<double> hessian; //P or H
    Eigen::VectorXd gradient;  //f or q
    Eigen::SparseMatrix<double> linearMatrix;  //A
    Eigen::VectorXd lowerBound; //l
    Eigen::VectorXd upperBound; //u

//下面的实例来源于 https://ww2.mathworks.cn/help/optim/ug/quadprog.html?s_tid=srchtitle
// 具有线性约束的二次规划
//     hessian.resize(2,2);
//     hessian.insert(0,0) = 1;
//     hessian.insert(1,0) = -1;
//     hessian.insert(0,1) = -1;
//     hessian.insert(1,1) = 2;
//     std::cout << "hessian:" << std::endl << hessian << std::endl;

//     gradient.resize(2);
//     gradient << -2, -6;
    
//     std::cout << "gradient:" << std::endl << gradient << std::endl;

//     linearMatrix.resize(3,2);
//     linearMatrix.insert(0,0) = 1;
//     linearMatrix.insert(0,1) = 1;
//     linearMatrix.insert(1,0) = -1;
//     linearMatrix.insert(1,1) = 2;
//     linearMatrix.insert(2,0) = 2;
//     linearMatrix.insert(2,1) = 1;
//     std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;

//     lowerBound.resize(3);
//     lowerBound << -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY;
//     std::cout << "lowerBound:" << std::endl << lowerBound << std::endl;

//     upperBound.resize(3);
//     upperBound << 2, 2, 3;
//     std::cout << "upperBound:" << std::endl << upperBound << std::endl;

//     int NumberOfVariables = 2; //A矩阵的列数
//     int NumberOfConstraints = 3; //A矩阵的行数

// 具有线性等式约束的二次规划
//     hessian.resize(2,2);
//     hessian.insert(0,0) = 1;
//     hessian.insert(1,0) = -1;
//     hessian.insert(0,1) = -1;
//     hessian.insert(1,1) = 2;
//     std::cout << "hessian:" << std::endl << hessian << std::endl;

//     gradient.resize(2);
//     gradient << -2, -6;
    
//     std::cout << "gradient:" << std::endl << gradient << std::endl;

//     linearMatrix.resize(1,2);
//     linearMatrix.insert(0,0) = 1;
//     linearMatrix.insert(0,1) = 1;
//     std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;

//     lowerBound.resize(1);
//     lowerBound << 0;
//     std::cout << "lowerBound:" << std::endl << lowerBound << std::endl;

//     upperBound.resize(1);
//     upperBound << 0;
//     std::cout << "upperBound:" << std::endl << upperBound << std::endl;

//     int NumberOfVariables = 2; //A矩阵的列数
//     int NumberOfConstraints = 1; //A矩阵的行数


//具有线性约束和边界的二次最小化
    hessian.resize(3,3);
    hessian.insert(0,0) = 1;
    hessian.insert(1,0) = -1;
    hessian.insert(2,0) = 1;
    hessian.insert(0,1) = -1;
    hessian.insert(1,1) = 2;
    hessian.insert(2,1) = -2;
    hessian.insert(0,2) = 1;
    hessian.insert(1,2) = -2;
    hessian.insert(2,2) = 4;
    std::cout << "hessian:" << std::endl << hessian << std::endl;

    gradient.resize(3);
    gradient << 2, -3, 1;
    
    std::cout << "gradient:" << std::endl << gradient << std::endl;

    linearMatrix.resize(4,3);
    linearMatrix.insert(0,0) = 1;
    linearMatrix.insert(1,0) = 0;
    linearMatrix.insert(2,0) = 0;
    linearMatrix.insert(3,0) = 1;

    linearMatrix.insert(0,1) = 0;
    linearMatrix.insert(1,1) = 1;
    linearMatrix.insert(2,1) = 0;
    linearMatrix.insert(3,1) = 1;

    linearMatrix.insert(0,2) = 0;
    linearMatrix.insert(1,2) = 0;
    linearMatrix.insert(2,2) = 1;
    linearMatrix.insert(3,2) = 1;
    std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;

    lowerBound.resize(4);
    lowerBound << 0, 0, 0, 0.5;
    std::cout << "lowerBound:" << std::endl << lowerBound << std::endl;

    upperBound.resize(4);
    upperBound << 1, 1, 1, 0.5;
    std::cout << "upperBound:" << std::endl << upperBound << std::endl;

    int NumberOfVariables = 3; //A矩阵的列数
    int NumberOfConstraints = 4; //A矩阵的行数



    // instantiate the solver
    OsqpEigen::Solver solver;

    // settings
    //solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    //矩阵A为m*n矩阵
    solver.data()->setNumberOfVariables(NumberOfVariables); //设置A矩阵的列数，即n
    solver.data()->setNumberOfConstraints(NumberOfConstraints); //设置A矩阵的行数，即m
    if(!solver.data()->setHessianMatrix(hessian)) return 1;//设置P矩阵
    if(!solver.data()->setGradient(gradient)) return 1; //设置q or f矩阵。当没有时设置为全0向量
    if(!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;//设置线性约束的A矩阵
    if(!solver.data()->setLowerBound(lowerBound)) return 1;//设置下边界
    if(!solver.data()->setUpperBound(upperBound)) return 1;//设置上边界

    // instantiate the solver
    if(!solver.initSolver()) return 1;

    Eigen::VectorXd QPSolution;

    // solve the QP problem
    if(!solver.solve()) return 1;

    // get the controller input
    QPSolution = solver.getSolution();

    std::cout << "QPSolution:" << std::endl << QPSolution << std::endl;
    std::cout << "QPSolution1221:" << std::endl;

    return 0;
}
